import sys
import os
if os.name == 'nt':
    import msvcrt
    def getch():
        return msvcrt.getch().decode()
        
else:
    import sys, tty, termios
    fd = sys.stdin.fileno()
    old_settings = termios.tcgetattr(fd)
    def getch():
        try:
            tty.setraw(sys.stdin.fileno())
            ch = sys.stdin.read(1)
        finally:
            termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
        return ch

sys.path.append("..")
from STservo_sdk import *    

class STServoController:
    def __init__(self, device_name='COM6', baudrate=1000000, servo_id=1):
        self.device_name = device_name
        self.baudrate = baudrate
        self.servo_id = servo_id
        self.port_handler = PortHandler(self.device_name)
        self.packet_handler = sts(self.port_handler)
        self.goal_positions = [0, 1023]
        self.moving_speed = 800
        self.moving_acc = 50
        self.index = 0


    def move_to_goal_position(self,goal_position):
        sts_comm_result, sts_error = self.packet_handler.WritePosEx(
            self.servo_id, 
            goal_position, 
            self.moving_speed, 
            self.moving_acc
        )
        if sts_comm_result != COMM_SUCCESS:
            raise Exception(self.packet_handler.getTxRxResult(sts_comm_result))
        if sts_error != 0:
            raise Exception(self.packet_handler.getRxPacketError(sts_error))

        while True:
            sts_present_position, sts_present_speed, sts_comm_result, sts_error = self.packet_handler.ReadPosSpeed(self.servo_id)
            if sts_comm_result != COMM_SUCCESS:
                raise Exception(self.packet_handler.getTxRxResult(sts_comm_result))
            if sts_error != 0:
                raise Exception(self.packet_handler.getRxPacketError(sts_error))

            print(f"[ID:{self.servo_id:03d}] GoalPos:{self.goal_positions[self.index]} PresPos:{sts_present_position} PresSpd:{sts_present_speed}")

            moving, sts_comm_result, sts_error = self.packet_handler.ReadMoving(self.servo_id)
            if sts_comm_result != COMM_SUCCESS:
                raise Exception(self.packet_handler.getTxRxResult(sts_comm_result))

            if moving == 0:
                break

        self.index = 1 - self.index

    def initialize(self):
        if not self.port_handler.openPort():
            raise Exception("Failed to open the port")
        if not self.port_handler.setBaudRate(self.baudrate):
            raise Exception("Failed to change the baudrate")
        else:
            self.move_to_goal_position(0)
        
    def close(self):
        self.port_handler.closePort()

# # Usage
# servo_controller = STServoController()
# try:
#     servo_controller.initialize()
#     servo_controller.move_to_goal_position(2500)
# finally:
#     servo_controller.close()